#include "pid.h"

/*
    pid初始化
*/
void pid_init(float kp, float ki, float kd, pid_t *pid)
{
    pid->actual_value = 0.0f;
    pid->error = 0.0f;
    pid->error_pre= 0.0f;
    pid->error_pre_pre = 0.0f;
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->set_value = 0.0f;
    pid->integral = 0.0f;
    pid->P = 0.0f;
    pid->I = 0.0f;
    pid->D = 0.0f;
}

/*
    位置式pid
    @param pid  要计算的pid
    @param set_value 想要设定的值
    返回 增量
*/
float pid_position(pid_t *pid)
{
    pid->error = pid->set_value - pid->actual_value;
    pid->integral += pid->error;   /*误差累积*/

    pid->P = pid->Kp*pid->error;    //比例项
    pid->I = pid->Ki*pid->integral; //积分项
    pid->D =pid->Kd*(pid->error-pid->error_pre);//微分项

    pid->error_pre_pre = pid->error_pre;
    pid->error_pre = pid->error;

    float uk = pid->P + pid->I +pid->D;
    pid->actual_value += uk;

    return uk;
}

/*
    增量式PID算法
    @param pid  要计算的pid
    @param set_value 想要设定的值 可以作为入参
    返回给执行器的输出值
*/
float pid_incremental(pid_t *pid)
{
    float P, I, D,increment;
    pid->error = pid->set_value - pid->actual_value;

    P = pid->Kp*(pid->error-pid->error_pre);
    I = pid->Ki*pid->error;
    D = pid->Kd*(pid->error-2*pid->error_pre + pid->error_pre_pre);
    
    increment = P + I + D;          /*pid计算得到的增量*/
    pid->actual_value += increment; /*实际反馈值加上增量*/

    pid->error_pre_pre = pid->error_pre;
    pid->error_pre = pid->error;

    return increment;
}
